#include<iostream>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <thread>
#include "MvCameraControl.h"
#include "start_cam.cpp"
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include <opencv2/video/video.hpp>
using namespace std;
using namespace cv;
const int para_time=100;
void catchfeature(Mat input, Mat &output,vector<Point2f> &feature,int thresh)
{
    Mat dst = Mat::zeros(input.size(), CV_32FC1);
    Mat raw = input.clone();
    vector<Point2f>corners;
    goodFeaturesToTrack(input, corners, thresh, 0.01, 10, Mat());
    for (int i = 0; i < corners.size(); i++)
    {
        circle(raw, corners[i], 2, Scalar(255, 0, 0), 2);
    }
    output = raw;
    feature = corners;
}
Mat Sharpen(Mat input, int percent, int type)
{
	Mat result;
	Mat s = input.clone();
	Mat kernel;
	switch (type)
	{
	case 0:
		kernel = (Mat_<int>(3, 3) <<
			0, -1, 0,
			-1, 4, -1,
			0, -1, 0
			);
	case 1:
		kernel = (Mat_<int>(3, 3) <<
			-1, -1, -1,
			-1, 8, -1,
			-1, -1, -1
			);
	default:
		kernel = (Mat_<int>(3, 3) <<
			0, -1, 0,
			-1, 4, -1,
			0, -1, 0
			);
	}
	filter2D(s, s, s.depth(), kernel);
	result = input + s * 0.01 * percent;
	return result;
}
void print_cam(void)
{
    Mat srcimg;
    Mat raw;
    Mat featurMat,old_raw;
    Mat im1, im2;
    int para_count=0;
    int para_flag=0;
    vector<Point2f>feature_pre;
    vector<Point2f>feature_after;
    vector<uchar> status;
    vector<float> err;

    int key;
    get_pic(&srcimg);
    get_pic(&old_raw);
    //srcimg=Mat(frame->stFrameInfo.nHeight,frame->stFrameInfo.nWidth,CV_8UC3,frame->pBufAddr);
    cvtColor(srcimg, im1, COLOR_BGR2GRAY);
    cvtColor(srcimg, im2, COLOR_BGR2GRAY);

 while(1){
     Mat img,Grey;

     try{
        get_pic(&srcimg);//RGB格式
        srcimg=Sharpen(srcimg,60,0);
        raw=srcimg.clone();
        cvtColor(srcimg,im1, COLOR_BGR2GRAY);
        catchfeature(im1, featurMat, feature_pre,512);
        calcOpticalFlowPyrLK(im1, im2, feature_pre, feature_after, status, err);

        int k = 0;
        for (int i = 0; i < feature_after.size(); i++)
        {
            //状态要是1，并且坐标要移动下的那些点
            if (status[i] && ((abs(feature_pre[i].x - feature_after[i].x) +
                abs(feature_pre[i].y - feature_after[i].y)) > 1))
            {
                feature_after[k++] = feature_after[i];
            }
        }
        feature_after.resize(k);//截取
        //cout << k << endl;
        for (int i = 0; i < feature_after.size(); i++)
        {
            //将特征点画一个小圆出来--粗细为2
            circle(old_raw, feature_after[i], 3, Scalar(0,255,255), 2);
        }

        namedWindow("srcImage", 0);

        imshow("srcImage", old_raw);
        imshow("featurMat", featurMat);
        im2 = im1.clone();
        old_raw=raw.clone();
        if(para_flag==1){
         set_cam_para();
         para_count++;
         usleep(5000);
         if(para_count>=para_time)
         {
            para_flag=0;
            para_count=0;
         }
        }

        //imshow("test4", srcimg);
        //usleep(20000);
        key=waitKey(10);
        if (key == 27){
            close_cam();
            sleep(1);
            destroyAllWindows();
            break;

        }else if(key==116){
            para_flag=1;
        }


}catch(const char *msg)
    {
        cout<<"sonthing wrong"<<endl;
        sleep(1);
    }
}

}

int main()
{

    Mat image;
    MV_FRAME_OUT Frame={0};
    memset(&Frame, 0, sizeof(MV_FRAME_OUT));
    //nRet = pthread_create(&nThreadID, NULL ,start_cam ,pf);
    start_cam();
    //thread t1(get_pic,&Frame);
    thread t2(print_cam);
    //t1.join();
    sleep(1);
    t2.join();


}
